/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package batailledesardennes.util.geomFloat.path;

import batailledesardennes.util.BinaryHeap;
import batailledesardennes.util.geomFloat.Point;
import batailledesardennes.util.geomFloat.Polygon;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

/**
 *
 * @author cghislai
 */
public class PathFinder {

    private Node startNode;
    private Node endNode;
    private final BinaryHeap<Node> openList;
    private NodeHandler nodeHandler;
    private final List<Node> toResetNodes;
    private GCostCalculator gcostCalculator;

    public PathFinder() {
        openList = new BinaryHeap<Node>();
        toResetNodes = new ArrayList<Node>();
        gcostCalculator = new DefaultGcostCalculator();
    }

    public List<Point> calcPath(Point start, Point end) {
        List<Point> points = new ArrayList<Point>();
        clearTempNodes();
        startNode = new Node();
        endNode = new Node();
        startNode.getPoint().setPos(start);
        endNode.getPoint().setPos(end);
        Node currentNode = startNode;

        // Move end if inside pol
        // Check straight path and move end if in polygon
        boolean intersection = false;
        for (Obstacle obstacle : nodeHandler.getObstacles()) {
            if (obstacle.getPolygon().containsPoint(end)) {
                Point p = obstacle.getPolygon().getClosestPoint(end, 1f);
                endNode.getPoint().setPos(p);
                end.setPos(p);
            }
            Polygon pol = obstacle.getPolygon();
            if (!pol.mayIntersectWithLine(start, end)) {
                continue;
            }
            if (pol.intersectsWithLineSegment(start, end)) {
                intersection = true;
            }
        }
        if (!intersection) {
            endNode.setParent(currentNode);
            fillPointList(points);
            return points;
        }
        // Connect start & end to nodes
        List<Node> connectableNodes = nodeHandler.getConnectableNodes(startNode);
        for (Node n : connectableNodes) {
            startNode.getTempNodes().add(n);
            n.getTempNodes().add(startNode);
        }
        connectableNodes = nodeHandler.getConnectableNodes(endNode);
        for (Node n : connectableNodes) {
            endNode.getTempNodes().add(n);
            n.getTempNodes().add(endNode);
        }
        // A*
        openList.makeEmpty();
        while (true) {
            currentNode.setPathStatus(Node.PathStatus.CLOSED);
            toResetNodes.add(currentNode);
            for (Node node : currentNode.getConnectedNodes()) {
                checkPathNode(currentNode, node);
            }
            for (Node node : currentNode.getTempNodes()) {
                checkPathNode(currentNode, node);
            }
            if (openList.isEmpty()) {
                // no path
                break;
            }
            currentNode = openList.peekMin();
            openList.deleteMin();
            if (currentNode == endNode) {
                fillPointList(points);
                break;
            }
        }
        clearTempNodes();
        startNode = null;
        endNode = null;
        return points;
    }

    private void clearTempNodes() {
        if (startNode != null) {
            for (Node node : startNode.getTempNodes()) {
                node.getTempNodes().clear();
            }
            startNode.getTempNodes().clear();
        }
        if (endNode != null) {
            for (Node node : endNode.getTempNodes()) {
                node.getTempNodes().clear();
            }
            endNode.getTempNodes().clear();
        }
        for (Node node : toResetNodes) {
            node.setPathStatus(null);
        }
    }

    private void checkPathNode(Node currentNode, Node node) {
        toResetNodes.add(node);
        if (node.getPathStatus() == null) {
            node.setParent(currentNode);
            node.setgCost(currentNode.getgCost() + calcGcost(currentNode, node));
            node.calcHCost(endNode);
            node.calcFCost();
            openList.add(node);
            node.setPathStatus(Node.PathStatus.OPEN);
            return;
        }
        if (node.getPathStatus() == Node.PathStatus.OPEN) {
            float gCost = node.getgCost();
            float newGCost = currentNode.getgCost() + calcGcost(currentNode, node);
            if (newGCost < gCost) {
                node.setParent(currentNode);
                node.setgCost(newGCost);
                node.calcFCost();
                int idx = openList.indexOf(node);
                openList.percolateUp(idx);
            }
        }
    }

    private void fillPointList(List<Point> points) {
        Node currentNode = endNode;
        while (true) {
            points.add(currentNode.getPoint());
            Node parentNode = currentNode.getParent();
            if (parentNode == null) {
                break;
            }
            currentNode = parentNode;
        }
        Collections.reverse(points);

    }

    private float calcGcost(Node fromNode, Node toNode) {
        return gcostCalculator.calcGCost(fromNode, toNode);
    }

    public void setNodeHandler(NodeHandler nodeHandler) {
        this.nodeHandler = nodeHandler;
    }

    public void setGcostCalculator(GCostCalculator gcostCalculator) {
        this.gcostCalculator = gcostCalculator;
    }

    private void debugStart() {
        System.out.println("============PATH LOOP=============");
        System.out.println("From " + pointString(startNode.getPoint()) + " to " + pointString(endNode.getPoint()));
    }

    private void debugCurrent(Node current) {
        System.out.println(" * current: " + pointString(current.getPoint()));
        printCosts(current);
    }

    private void debugCheck(Node node) {
        System.out.println("  - check against : " + pointString(node.getPoint()) + " with status " + statusString(node));
        printCosts(node);
    }

    private void debugCheckNew(Node node) {
        System.out.println("     new node");
        printCosts(node);
    }

    private void debugCheckBetter(Node node) {
        System.out.println("     better one");
        printCosts(node);
    }

    private void printCosts(Node node) {
        System.out.println("        gCost: " + node.getgCost());
        System.out.println("        hCost: " + node.gethCost());
        System.out.println("        fCost: " + node.getfCost());
    }

    private String pointString(Point p) {
        return "(" + p.getX() + "," + p.getY() + ")";
    }

    private String statusString(Node node) {
        if (node.getPathStatus() == null) {
            return "UNSET";
        }
        return node.getPathStatus().name();
    }

    private final class DefaultGcostCalculator implements GCostCalculator {

        @Override
        public float calcGCost(Node fromNode, Node toNode) {
            return fromNode.getPoint().distance(toNode.getPoint());
        }
    }
}
